#include "keyboard.h"
#include "usbd_def.h"

extern UART_HandleTypeDef huart1;

extern uint8_t TXData[20];

int8_t get_key_status(KEYBOARD_KEY_INDEX index)
{
    int8_t ret = -1;
    switch (index)
    {
    case KEYBOARD_KEY_UP:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_6);
        break;
    case KEYBOARD_KEY_DOWN:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_9);
        break;
    case KEYBOARD_KEY_LEFT:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_8);
        break;
    case KEYBOARD_KEY_RIGHT:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_7);
        break;
    case KEYBOARD_KEY_PLUS:
        ret = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_15);
        break;
    case KEYBOARD_KEY_O:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3);
        break;
    case KEYBOARD_KEY_OK:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4);
        break;
    case KEYBOARD_KEY_SUB:
        ret = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5);
        break;
    case KEYBOARD_KEY_POWER_STATUS:
        ret = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6);
        break;
    case KEYBOARD_KEY_MENU:
        ret = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_8);
        break;
    default:
        break;
    }

    return ret;
}

void power_key_ctrl(KEYBOARD_POWER_KEY cmd)
{
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, cmd);
}

int32_t keyboard_init()
{
    // keyboard left gpiob 9 8 7 6                6:up 7:right 8:left 9:down
    // keyboard right gpiob 5 4 3, gpioa 15       gpioa-15:+  3:O  4:OK  5:-
    // keyboard center gpioa  8                   gpioa-8:menu
    // power_key_det:gpioa-6  power_key_ctrl:gpioa-7
    __HAL_RCC_GPIOA_CLK_ENABLE(); // 启用GPIOA时钟
    __HAL_RCC_GPIOB_CLK_ENABLE(); // 启用GPIOB时钟

    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_8 | GPIO_PIN_15;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; // 设置模式为输入
    GPIO_InitStruct.Pull = GPIO_PULLUP;          // 上拉电阻
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);      // 初始化GPIOA的0号引脚

    GPIO_InitStruct.Pin = GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; // 设置模式为输出
    GPIO_InitStruct.Pull = GPIO_PULLUP;         // 上拉电阻
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);     // 初始化GPIOA的0号引脚

    GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8 | GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; // 设置模式为输入
    GPIO_InitStruct.Pull = GPIO_PULLUP;          // 上拉电阻
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);      // 初始化GPIOB的0号引脚

    // /* 使能并配置中断 */
    /* EXTI interrupt init*/
    HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(EXTI3_IRQn);

    HAL_NVIC_SetPriority(EXTI4_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(EXTI4_IRQn);

    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);

    HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);

    power_key_ctrl(POWER_KEY_ON);
    return 0;
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{

    switch (GPIO_Pin)
    {
    case GPIO_PIN_3:
        get_key_status(KEYBOARD_KEY_O);
        break;
    case GPIO_PIN_4:
        get_key_status(KEYBOARD_KEY_OK);
        break;
    case GPIO_PIN_5:
        get_key_status(KEYBOARD_KEY_SUB);
        break;
    case GPIO_PIN_6:
        if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) == GPIO_PIN_RESET)
        {
            // GPIOA触发
            get_key_status(KEYBOARD_KEY_POWER_STATUS);
        }
        else
        {
            // GPIOB触发，因为使用了相同的回调函数，所以需要其他方法区分
            get_key_status(KEYBOARD_KEY_UP);
        }
        break;
    case GPIO_PIN_7:
        get_key_status(KEYBOARD_KEY_RIGHT);
        break;
    case GPIO_PIN_8:
        get_key_status(KEYBOARD_KEY_LEFT);
        break;
    case GPIO_PIN_9:
        get_key_status(KEYBOARD_KEY_DOWN);
        break;
    case GPIO_PIN_15:
        get_key_status(KEYBOARD_KEY_PLUS);
        break;
    }
}

/*	Update all buttons with a single array
 *		Order is as follows A,B,X,Y,LB,RB,L3,R3,START,BACK,LOGO
 *		11 buttons 0-10 in the array
 */
void XINPUT_buttonArrayUpdate(uint8_t buttonArray[11])
{
    // BUTTON_A
    if (buttonArray[0])
    {
        TXData[BUTTON_PACKET_2] |= A_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= A_MASK_OFF;
    }
    // BUTTON_B
    if (buttonArray[1])
    {
        TXData[BUTTON_PACKET_2] |= B_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= B_MASK_OFF;
    }
    // BUTTON_X
    if (buttonArray[2])
    {
        TXData[BUTTON_PACKET_2] |= X_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= X_MASK_OFF;
    }
    // BUTTON_Y
    if (buttonArray[3])
    {
        TXData[BUTTON_PACKET_2] |= Y_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= Y_MASK_OFF;
    }
    // BUTTON_LB
    if (buttonArray[4])
    {
        TXData[BUTTON_PACKET_2] |= LB_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= LB_MASK_OFF;
    }
    // BUTTON_RB
    if (buttonArray[5])
    {
        TXData[BUTTON_PACKET_2] |= RB_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= RB_MASK_OFF;
    }
    // BUTTON_L3
    if (buttonArray[6])
    {
        TXData[BUTTON_PACKET_1] |= L3_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_1] &= L3_MASK_OFF;
    }
    // BUTTON_R3
    if (buttonArray[7])
    {
        TXData[BUTTON_PACKET_1] |= R3_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_1] &= R3_MASK_OFF;
    }
    // BUTTON_START
    if (buttonArray[8])
    {
        TXData[BUTTON_PACKET_1] |= START_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_1] &= START_MASK_OFF;
    }
    // BUTTON_BACK
    if (buttonArray[9])
    {
        TXData[BUTTON_PACKET_1] |= BACK_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_1] &= BACK_MASK_OFF;
    }
    // BUTTON_LOGO
    if (buttonArray[10])
    {
        TXData[BUTTON_PACKET_2] |= LOGO_MASK_ON;
    }
    else
    {
        TXData[BUTTON_PACKET_2] &= LOGO_MASK_OFF;
    }
}

/* 	Update dpad values in the packet
 *		SOCD cleaner included
 *		Programmed behavior is UP+DOWN=UP and LEFT+RIGHT=NEUTRAL
 *		SOCD makes fightsticks tournament legal and helps prevent erroneous states
 */
void XINPUT_dpadUpdate(uint8_t dpadUP, uint8_t dpadDOWN, uint8_t dpadLEFT, uint8_t dpadRIGHT)
{
    // Clear DPAD
    TXData[BUTTON_PACKET_1] &= DPAD_MASK_OFF;
    // DPAD Up
    if (dpadUP)
    {
        TXData[BUTTON_PACKET_1] |= DPAD_UP_MASK_ON;
    }
    // DPAD Down
    if (dpadDOWN && !dpadUP)
    {
        TXData[BUTTON_PACKET_1] |= DPAD_DOWN_MASK_ON;
    }
    // DPAD Left
    if (dpadLEFT && !dpadRIGHT)
    {
        TXData[BUTTON_PACKET_1] |= DPAD_LEFT_MASK_ON;
    }
    // DPAD Right
    if (dpadRIGHT && !dpadLEFT)
    {
        TXData[BUTTON_PACKET_1] |= DPAD_RIGHT_MASK_ON;
    }
}

/*	Update the trigger values in the packet
 *		0x00 to 0xFF
 */
void XINPUT_triggerUpdate(uint8_t triggerLeftValue, uint8_t triggerRightValue)
{
    TXData[LEFT_TRIGGER_PACKET] = triggerLeftValue;
    TXData[RIGHT_TRIGGER_PACKET] = triggerRightValue;
}

/*	Update a single trigger value in the packet
 *		0x00 to 0xFF
 */
void XINPUT_singleTriggerUpdate(uint8_t trigger, uint8_t triggerValue)
{
    if (trigger == TRIGGER_LEFT)
    {
        TXData[LEFT_TRIGGER_PACKET] = triggerValue;
    }
    else if (trigger == TRIGGER_RIGHT)
    {
        TXData[RIGHT_TRIGGER_PACKET] = triggerValue;
    }
    else
    { /*invalid parameter*/
    }
}

/*	Analog Sticks
 *		Each axis is a signed 16 bit integer
 *		-32,768 to 32,767 is the range of value
 */
void XINPUT_stickUpdate(uint8_t analogStick, int16_t stickXDirValue, int16_t stickYDirValue)
{
    if (analogStick == STICK_LEFT)
    {
        // Left Stick X Axis
        TXData[LEFT_STICK_X_PACKET_LSB] = LOBYTE(stickXDirValue); // (CONFERIR)
        TXData[LEFT_STICK_X_PACKET_MSB] = HIBYTE(stickXDirValue);
        // Left Stick Y Axis
        TXData[LEFT_STICK_Y_PACKET_LSB] = LOBYTE(stickYDirValue);
        TXData[LEFT_STICK_Y_PACKET_MSB] = HIBYTE(stickYDirValue);
    }
    else if (analogStick == STICK_RIGHT)
    {
        // Right Stick X Axis
        TXData[RIGHT_STICK_X_PACKET_LSB] = LOBYTE(stickXDirValue);
        TXData[RIGHT_STICK_X_PACKET_MSB] = HIBYTE(stickXDirValue);
        // Right Stick Y Axis
        TXData[RIGHT_STICK_Y_PACKET_LSB] = LOBYTE(stickYDirValue);
        TXData[RIGHT_STICK_Y_PACKET_MSB] = HIBYTE(stickYDirValue);
    }
    else
    { /*invalid parameter*/
    }
}

/*	Read/update buttons and potentiometer of the handrake
 *
 */
void readButtons()
{

    int state = 1;
    uint8_t buttonArray[11] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // initialize array of buttons
    uint8_t dpadArray[4] = {0, 0, 0, 0};                         // initialize array from dpad

    for (int i = KEYBOARD_KEY_PLUS; i < KEYBOARD_KEY_MAX; i++)
    {
        // 0 = PRESSED		1 = NOT PRESSED
        state = get_key_status(i); // read buttons, button is active-low
        buttonArray[i - KEYBOARD_KEY_PLUS] = !state;
    }
    // static char buf[50];
    // sprintf(buf, "%d, %d, %d, %d, %d, %d\r\n", buttonArray[0], buttonArray[1], buttonArray[2], buttonArray[3], buttonArray[4], buttonArray[5]);
    // HAL_UART_Transmit_IT(&huart1, (uint8_t *)buf, strlen(buf));
 
    XINPUT_buttonArrayUpdate(buttonArray); // update buttons

    for (int i = KEYBOARD_KEY_UP; i < KEYBOARD_KEY_PLUS; i++)
    {
        // 0 = PRESSED		1 = NOT PRESSED
        state = get_key_status(i); // read buttons, button is active-low
        dpadArray[i] = !state;
    }

    XINPUT_dpadUpdate(dpadArray[0], dpadArray[1], dpadArray[2], dpadArray[3]); // update dpad
}

#define STICK_16_MAX 50000
#define STICK_16_MIN -50000

#define INT16MAX 32767
#define INT16MIN -32767

int8_t adcValueReady = 0;
// Initiate
int16_t wheelEncoderValue = 0;
uint16_t handbrakeValue = 0;
int16_t leftTriggerValue_ADC = 0;
int16_t rightTriggerValue_ADC = 0;
int16_t xLeftStickValue_ADC = 0;
int16_t yLeftStickValue_ADC = 0;
int16_t xRightStickValue_ADC = 0;
int16_t yRightStickValue_ADC = 0;

int16_t leftTriggerValue = 0;
int16_t rightTriggerValue = 0;
int16_t xLeftStickValue = 0;
int16_t yLeftStickValue = 0;
int16_t xRightStickValue = 0;
int16_t yRightStickValue = 0;
/*	Re-maps a number from one range to another
 *
 */
int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int32_t xinput_restrict(int32_t x, int32_t max, int32_t middle)
{
    if (max > middle)
    {
        if (x > max)
        {
            return max;
        }
        else
        {
            return x;
        }
    }
    else
    {
        if (x < max)
        {
            return max;
        }
        else
        {
            return x;
        }
    }
}

/*	Update brake and throttle triggers
 *
 */
void updateTriggers()
{
    XINPUT_triggerUpdate(leftTriggerValue, rightTriggerValue);
}

/*	Update right and left sticks
 *			LeftStick X-axis comes from steering wheel
 *			RightStick comes from potentiometers
 */
void updateSticks()
{

    XINPUT_stickUpdate(STICK_LEFT, xLeftStickValue, yLeftStickValue);
    XINPUT_stickUpdate(STICK_RIGHT, xRightStickValue, yRightStickValue);
}

void readAdcValues()
{
    if (adcValueReady == 1)
    {
        rightTriggerValue = (uint8_t)map(0, 0, 4040, 0, UINT8_MAX);
        leftTriggerValue = (uint8_t)map(0, 0, 4040, 0, UINT8_MAX);

        if (xLeftStickValue_ADC < 2020)
        {
            int32_t tmp = xinput_restrict(xLeftStickValue_ADC, 752, 2020);
            xLeftStickValue = (int16_t)map(tmp, 752, 2020, INT16MIN, 0);
        }
        else
        {
            int32_t tmp = xinput_restrict(xLeftStickValue_ADC, 3261, 2020);
            xLeftStickValue = (int16_t)map(tmp, 2020, 3261, 0, INT16MAX);
        }

        if (yLeftStickValue_ADC < 2020)
        {
            int32_t tmp = xinput_restrict(yLeftStickValue_ADC, 895, 2020);
            yLeftStickValue = (int16_t)map(tmp, 895, 2020, INT16MAX, 0);
        }
        else
        {
            int32_t tmp = xinput_restrict(yLeftStickValue_ADC, 3211, 2020);
            yLeftStickValue = (int16_t)map(tmp, 2020, 3211, 0, INT16MIN);
        }

        if (xRightStickValue_ADC < 2020)
        {
            int32_t tmp = xinput_restrict(xRightStickValue_ADC, 816, 2020);
            xRightStickValue = (int16_t)map(tmp, 816, 2020, INT16MAX, 0);
        }
        else
        {
            int32_t tmp = xinput_restrict(xRightStickValue_ADC, 3225, 2020);
            xRightStickValue = (int16_t)map(tmp, 2020, 3225, 0, INT16MIN);
        }

        if (yRightStickValue_ADC < 2020)
        {
            int32_t tmp = xinput_restrict(yRightStickValue_ADC, 922, 2020);
            yRightStickValue = (int16_t)map(tmp, 922, 2020, INT16MIN, 0);
        }
        else
        {
            int32_t tmp = xinput_restrict(yRightStickValue_ADC, 3185, 2020);
            yRightStickValue = (int16_t)map(tmp, 2020, 3185, 0, INT16MAX);
        }

        adcValueReady = 0;
    }
}
